#include <ContourEQW.h>
#include <szMexUtility.h>

ContourEQW::ContourEQW(int n):X(n,0), Y(n,0), A(n,0), B(n,0), C(n,0), D(n,0)
{
}

ContourEQW::ContourEQW(Contour c)
{
	int n = c.points.size();
	X.resize(n);
	Y.resize(n);
	A.resize(n);
	B.resize(n);
	C.resize(n);
	D.resize(n);
	for(int i=0; i<c.points.size(); ++i)
	{
		X[i] = c.points[i].m_X;
		Y[i] = c.points[i].m_Y;
	}
}

ContourEQW
ContourEQW::operator =(Contour c)
{
	int n = c.points.size();
	X.resize(n);
	Y.resize(n);
	A.resize(n);
	B.resize(n);
	C.resize(n);
	D.resize(n);
	for(int i=0; i<n; ++i)
	{
		X[i] = c.points[i].m_X;
		Y[i] = c.points[i].m_Y;
		A[i] = 0;
		B[i] = 0;
		C[i] = 0;
		D[i] = 0;
	}
	return *this;
}

vector<float>
ContourEQW:: _FindOptimumPoint(int n0, int n2) const 
{
	float x0[3]={X[n0], A[n0], B[n0]};
	float x2[3]={X[n2], A[n2], B[n2]};
	float v[3],x1[3];
	int i,j;
	for(i=0; i<3; ++i) 
	{
		float sum=.0;
		for(j=0; j<3; ++j) 
		{
			sum+=H[i][j]*x2[j]+H[j][i]*x0[j];
		}
		v[i]=sum;
	}
	for(i=0; i<3; ++i) 
	{
		float sum=.0;
		for(j=0; j<3; ++j) 
		{
			sum+=G[i][j]*v[j];
		}
		x1[i]=-sum;
	}
	vector<float> res(6);
	res[0]=x1[0];
	res[2]=x1[1];
	res[3]=x1[2];

	float y0[3]={Y[n0], C[n0], D[n0]};
	float y2[3]={Y[n2], C[n2], D[n2]};
	float u[3],y1[3];
	for(i=0; i<3; ++i) 
	{
		float sum=.0;
		for(j=0; j<3; ++j) 
		{
			sum+=H[i][j]*y2[j]+H[j][i]*y0[j];
		}
		u[i]=sum;
	}
	for(i=0; i<3; ++i) 
	{
		float sum=.0;
		for(j=0; j<3; ++j) 
		{
			sum+=G[i][j]*u[j];
		}
		y1[i]=-sum;
	}
	res[1]=y1[0];
	res[4]=y1[1];
	res[5]=y1[2];

	return res;	
}

void
ContourEQW::Smoothen(int niter, float r, float t, bool bClosed)
{
	float a=4*r+t*t, b=8*r+t*t, c=2*r+t*t;
	G[0][0] = .5*a/b; G[0][1]=-.5/b; G[0][2] = 0;
	G[1][0] = -.5/b; G[1][1] = 1./(t*t*b); G[1][2] = 0;
	G[2][0] = 0; G[2][1] = 0; G[2][2] = .5/c;
	H[0][0] = -2.; H[0][1]=-t*t; H[0][2] = t;
	H[1][0] = -t*t; H[1][1] = 0; H[1][2] = -2.*r*t;
	H[2][0] = -t; H[2][1] = 2.*r*t; H[2][2] = -2.*r;

	int nump=X.size();
	int id1, id2;
	if(bClosed) 
	{
		id1=0;
		id2=nump-1;
	}
	else //fix the end point
	{
		id1=1;
		id2=nump-2;
	}
	for(int i=0; i<niter; ++i)
	{
		for(int j=id1; j<=id2; ++j)
		{
			int j0=(j - 1 + nump) % nump;
			int j2=(j + 1) % nump;
			vector<float> res = _FindOptimumPoint(j0, j2);
			X[j] = res[0];
			Y[j] = res[1];
			A[j] = res[2];
			B[j] = res[3];
			C[j] = res[4];
			D[j] = res[5];
		}
	}
}

vector<float>
ContourEQW::Curvatures()
{
	vector<float> curvatures(X.size());
	for(int i=0; i<X.size(); ++i)
	{
		float dd = pow(B[i]*B[i]+D[i]*D[i], (float)1.5);
		curvatures[i] = (B[i]*C[i] - A[i]*D[i])/Max(0.00001,dd);
	}
	return curvatures;
}

